//MAP
//Implementation

#include "map.hpp"
#include "ros/ros.h"

int& MapCol::operator [](int y) {
	int yWithOffset = y+offset;
	if(yWithOffset < 0) {
        //std::cout << "MapCol: " << y << " is outside the lower boundary, extending. Offset: " << offset << "... " << std::endl;
		while(yWithOffset < 0) {
			data.push_front(-1);
			offset++;
            yWithOffset = y+offset;
		}
	}
	else {
        if(yWithOffset >= (int)data.size()) {
            //std::cout << "MapCol: " << y << " is outside the upper boundary, extending. Offset: " << offset << std::endl;
            while(yWithOffset >= (int)data.size()) {
				data.push_back(-1);
			}
		}
	}
	return data[yWithOffset];
}

int MapCol::get(int y) {
    if(y+offset < 0 || y+offset >= (int)data.size() ) {
		return -1;
	}
	return data[y+offset];
}

MapCol::MapCol() {
	data.push_back(-1);
	offset = 0;
}

MapCol& Map::operator [](int x) {
	
	int xWithOffset = x+offset;
	
	if(xWithOffset < 0) {
        //std::cout << "Map: " << x << " is outside the lower boundary, extending. Offset: " << offset << "... ";
		while(xWithOffset < 0) {
			MapCol column;
			data.push_front(column);
			offset++;
            xWithOffset = x+offset;
		}
		std::cout << std::endl;
	}
	else {
        if(xWithOffset >= (int)data.size()) {
            //std::cout << "Map: " << x << " is outside the upper boundary, extending. Offset: " << offset << std::endl;
            while(xWithOffset >= (int)data.size()) {
				MapCol column;
				data.push_back(column);
			}
		}
	}
	return data[xWithOffset];
}

MapCol& Map::get(int x) {
    if(x+offset < 0 || x+offset >= (int)data.size()) {
        static MapCol dummy;
		return dummy;
	}
	return data[x+offset];
}

Map::Map() {
	MapCol column;
	data.push_back(column);
	offset = 0;
}

void Map::generateMap(nav_msgs::OccupancyGrid& target) {
	//Determine min and max Y values
	
    target.data.clear();

    int minY = 0;
    int maxY = 0;
	
	ROS_INFO("Calculating Y borders and size...");
	
    for(unsigned int i=0; i<data.size(); i++) {
		if(-data[i].offset < minY) {
			minY = -data[i].offset;
		}
        if((int)data[i].data.size()-data[i].offset > maxY) {
			maxY = data[i].data.size()-data[i].offset;
		}
	}
	
	//Calculate the future size of the map and resize it
	int mapHeight = maxY - minY;	
	int mapSize = mapHeight * data.size();
	
	ROS_INFO("Y borders: %d, %d, map height: %d, map size: %d", minY, maxY, mapHeight, mapSize);
	
	target.data.resize(mapSize);
    for(unsigned int i=0; i<data.size(); i++) {
        for(int y=minY; y<maxY; y++) {
            target.data[(y-minY)*data.size()+i] = ((data[i].get(y) < 0) ? -1 : ((data[i].get(y) > 40) ? 100 : 0));
		}
	}
	
	ROS_INFO("Values written into target.");
	
	target.info.width = data.size();
	target.info.height = mapHeight;
}
